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Abstract: Continuation model predictive control (MPC), introduced by T. Ohtsuka in 2004, 
uses Krylov-Newton approaches to solve MPC optimization and is suitable for nonlinear and 
minimum time problems. We suggest particle continuation MPC in the case, where the system 
dynamics or constraints can discretely change on-line. We propose an algorithm for on-line 
controller implementation of continuation MPC for ensembles of predictions corresponding to 
various anticipated changes and demonstrate its numerical effectiveness for a test minimum 
time problem arriving to a destination. Simultaneous on-line particle computation of ensembles 
of controls, for several dynamically changing system dynamics, allows choosing the optimal 
destination on-line and adapt it as needed. 
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1. INTRODUCTION 

Model predictive control (MPC) is a popular control 
approach, which efficiently treats constraints on state and 
control variables. Solid introduction into MPC is found in 
Camacho et al. (2004) and Griine et al. (2011), industrial 
applications are discussed in Qin et al. (2003), numerical 
aspects of MPC are surveyed in Diehl et al. (2009) and 
Wang et al. (2010). 

Ohtsuka (2004) has developed an on-line numerical method 
for nonlinear MPC, based on the so-called Newton-Krylov 
method; see Knoll et al. (2004) for other applications of 
the Newton-Krylov method. Knyazev et al. (2015) provide 
an efficient preconditioner for the Newton-Krylov method 
and extend the MPC model, treated in Ohtsuka (2004), to 
the minimum-time problem. 

In the present note, we demonstrate how the precondi¬ 
tioned Newton-Krylov method by Knyazev et al. (2015) 
works in cases, where the system dynamics or constraints 
can discretely change on-line. The problem with discrete 
switches requires simultaneous solution to several finite- 
horizon predictions, which can be done independently on 
parallel processors. The computer time can be also reduced 
by using the same preconditioner for all finite-horizon 
control problems, if the discrete changes of the system 
dynamics or constraints do not lead to large norms of the 
residual mapping F, introduced in Section 2. If the norm 
||F|| is not small enough after a discrete switch, the method 
may be even ruined during execution. Such a behavior 
is inherited from the Newton method, which converges 
only for sufficiently good initial guess. The finite-horizon 
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predictions with HUH larger than a suitable tolerance may 
be discarded or refined. 

The necessity to compute several close trajectories also 
appears in the so called particle control problems; see e.g. 
Ohsumi et al. (2011). 

The rest of the note is organized as follows. Section 2 
presents a framework of the receding horizon prediction 
problem and obtains its solution in the form of a nonlinear 
equation. Section 3 discusses how this nonlinear equation 
is efficiently solved for an ensemble of sufficiently close 
trajectories issued from the current state. Section 4 de¬ 
scribes a test example and presents formulas for computer 
implementation. Section 5 shows numerical results. 

2. RECEDING HORIZON PREDICTION 

In this section, we introduce the receding horizon problem 
for an MPC model considered in Knyazev et al. (2015). 
The finite horizon is an interval [t,t-\-T], where T may de¬ 
pend on t. The control input u{t) and parameter vector p 
are determined so as to minimize the performance index 

t-l-T 

J = (l}{x{t-\-T),p)-\- j L{T,x{T),u(T),p)dT 

t 

subject to the state dynamics 

= f{T,x{T),u{T),p), (1) 

and the equality constraints for the state x and the 
control u 

C{t,x{t),u{t),p) =0, (2) 

/{x{t-\-T),p) = 0. (3) 





The initial value condition x{T)\r=t for equation (1) is 
the state vector x{t) of the dynamic system. The control 
vector u = u{T)\r=t, solving the problem over the receding 
horizon, is used afterwards as an input to control the 
dynamic system at time t. 

Let us discretize the continuous control problem stated 
above on a time grid Ti, i = 0,1,..., N, obtained by 
partitioning the horizon [t, t+T] into N subintervals of size 
= Ti+i — Ti. The vector functions x{t) and u{t) are 
replaced by their values Xi and Ui at the grid points Tj. The 
integral in the performance index J is approximated by the 
rectangular quadrature rule. Equation (1) is integrated by 
the explicit Euler method. The discretized optimal control 
problem is as follows: 

N-l 

min 4>{xn,p) + y^L{Ti,Xi,Ui,p)ATi , 

Ui .P ' ^ 

L J 


subject to 

Xi+i = Xi + f{Ti,Xi,Ui,p)ATi, i = 0,1,... ,N - 1, (4) 
C{Ti,Xi,Ui,p) = 0, i = 0,l,...,N -1, (5) 

'ip{xN,p) = 0. (6) 


compute the costates Xi, i = N—1, ..., 0, by the backward 
recursion 


Xi — Ai+i + 


dH^ 

dx 


{n ,Xi,X^+i,Ui,fj,i, p) An. 


(2) Combine the control input u, the Lagrange multiplier 
p, the Lagrange multiplier v, and the parameter p, all in 
one vector 

U(t) = [u^,... ,..., ,p'^f. 


Calculate the mapping F[U,x,t], using just obtained val¬ 
ues Xi and Xi, as 


F[U,x,t] 


dH^ 

du 

dH^ 

du 


(to,xo, Ai,mo,Mo,p)Ato 


{Ti,X^,Xi+l,Ui,p^,p)ATi 


dH^ 

du 


{tn-i,xn-i,Xn,un-i, Pn-i,p)Atn-i 


The necessary optimality conditions for the discretized 
finite horizon problem are obtained by means of the 
discrete Lagrangian function 


N-l 

C{X,U) = (l){xN,p) + ^ L{Ti,Xi,Ui,p)XTi 

iV-1 

+ Ao [x(t) -Xo] + ^ -Xi+l ^ ^i{Ti,Xi,Ui,p)XTi] 

iV-1 

+ ^ piJC{Ti,Xi,Ui,p)XTi + lJ^'il){xN,p)-, 

where X = [xi Aj]^, i = 0,1,... ,N, and U = [ui pi v p]^, 
i = 0,l,...,fV—1. Here, A is the costate vector and p is the 
Lagrange multiplier vector associated with constraint (5). 
The terminal constraint (6) is relaxed by the aid of the La¬ 
grange multiplier u. The necessary optimality conditions 
are the KKT stationarity conditions: jCx^ = 0, Cxi = 0, 
i = 0,1,..., N, Cuj = 0, = 0, i = 0,1,..., - 1, 

= 0, /Ip, = 0. 

For convenience in the subsequent formulations, we intro¬ 
duce the Hamiltonian function 


C'(To,xo,'Uo,p)Aro 

C{Ti,Xi,Ui,p)ATi 

C(tn-i,xn-i,un-i, p)Atn-i 


')pixN,p) 

d<f d-ip^ 

-(XN,P) + -T—[XN,p)v 


N-l 


dp 


dp 


-nn-iF,Xi,Xi+i,Ui,pi,p)ATi 


i=0 


dp 


The vector argument x in F[U,x,t] denotes the initial 
vector xq in the forward recursion. 

The equation with respect to the unknown vector U(t) 
F[U(t),x(t),t] = 0 


gives the required necessary optimality conditions that are 
solved on the controller in real time. 


3. NUMERICAL ALGORITHM 


H{t, X, X, u, p,p) = L(t, X, u,p) 

+ X^f{t, X, u,p) + p^C{t, X, u,p). 

The number of unknowns in the KKT conditions can be 
reduced by eliminating the states Xi and costates Xp 

(1) Starting from the current measured or estimated state 
xo, compute a;i,i = 0,l...,A^—l,by the forward recursion 

Xi+i = Xi + f{Ti,Xi,Ui,p)ATi. 


Then starting from 

d(j)^ 


Xn = 


dx 


{XN,P) 


dx 


{XN,P)V 


The controlled dynamic system is sampled on a discrete 
time grid tj, j = 0,1,.... The sampled values of the state 
and parameters are Xj = x(tj) and Uj = U{tj). Choosing 
a small h, which is usually much less than Atj = tj+i — tj 
and Axk, we introduce the operator 

aj{V) = {F[Uj-i + hV,Xj,tj] - F[Uj-i,Xj,tj])/h. 

The discrete equation F[Uj,Xj,tj] = 0 is then equivalent 
to the operator equation aj{AUj/h) = bj/h, where AUj = 
Uj — Uj-i, bj = —F[Uj-i,Xj,tj]. This operator equation 
allows us to compute Uj, if Uj-i is known. 

Let us denote the fc-th column of the mxm identity matrix 
by Cfe, where m is the dimension of the vector U, and form 



an m X m matrix Aj with the columns AjCk = aj{ek), 
k = 1,... ,m. The matrix Aj is an 0{h) approximation of 
the Jacobian matrix Fir[Uj-i,Xj,tj], which is symmetric. 

We suppose that Uq is an approximate solution to the 
equation F[Uo,xo,to] = 0 and omit discussion of methods 
for computing Uq. The hrst block entry of Uq is taken as 
the control uq at the state xq- The next state xi = x(ti) 
is either sensor estimated or computed by the formula 
Xi = xo + /(to, Xo, uo)Ato; cf. (4). 

At the time j > 1, we have the state xj and the vector 
Uj-i evaluated at the previous time tj-i. We solve the 
following operator equation with respect to V : 

aj{V)=bj/h. (7) 

Then we set AUj = hV, Uj = Uj-i + AUj and choose the 
first block component of Uj as the control Uj. The next 
system state Xj+i = x(tj+i) is either sensor estimated or 
computed by the formula Xj+i = Xj + f{tj,Xj,Uj)Atj. 

An approximate solution to (7) can be found by computing 
the matrix Aj and then solving the system of linear 
equations AjAUj = hj by the Gaussian elimination with 
pivoting. A more efficient way is solving (7) by the GMRES 
method, where the operator aj{V) is used instead of Aj as 
in Ohtsuka (2004) and Knyazev et al. (2015). 

Gonvergence of GMRES can be accelerated by precondi¬ 
tioning. A matrix M that is close to the matrix A and 
such that computing M~^r for an arbitrary vector r is 
relatively easy, is referred to as a preconditioner. The 
preconditioning for the system of linear equations Ax = b 
with the preconditioner M formally replaces the original 
system Ax = b with the equivalent preconditioned linear 
system M~^Ax = M~^b. When the condition number 
||M“^A|| II is sufficiently small, convergence of the 

preconditioned GMRES is fast. The vector z = M~^r is 
often computed via back-substitutions as z = 
where L and U are the triangular factors in the LU factor¬ 
ization M = LU computed by the Gaussian elimination. 

Knyazev et al. (2015) compute the matrix Aj exactly for 
some time instances tj and use it as a preconditioner for 
GMRES at a number of subsequent time instances tj, tj+i, 

..., . 

In the present paper, we suggest to apply the above 
described numerical method in more general situations, 
where the dynamic system and/or constraints depend on 
a discrete parameter, or switch, with few values 1 , ..., q. 
In other words, there are q functions f^'^\t,x,u,p) and q 
mappings F^'^'>{U,x,t), k = I, 2,..., < 7 , and at each time 
instance tj we must select k such that the performance 
index is minimized. 

The numerical method is modified as follows. At the time 
tj, j > 1, the system state is given by Xj, and we have 
the vector Uj-i evaluated at the previous time tj-i. Since 
there are q mappings we must solve q operator 

equations with respect to V: 

af\v^'^'>) = bf^/h. ( 8 ) 

Solutions with low precision can be discarded or 

refined. In the set of admissible solutions we select 
the solution with minimum performance index. Then 


we set AUj = Uj = Uj-i + AUj and choose the 

first block component of Uj as the control Uj. The next 
system state Xj+i = x{tj+i) is either sensor estimated or 
computed by the formula Xj+i = Xj + f'^^^\tj,Xj,Uj)Atj. 

Equations (8) are solved by GMRES independently. How¬ 
ever, a preconditioner for all k can be a single matrix Aj, 
which is evaluated for some suitable k. 

The vector Uj-i satishes F^’^^-'^''{Uj-i,Xj-i,tj-i) w 0. 
When each of the q equations F^'^^ (Uj'^\xj,tj) = 0 is 
solved by the Newton method, it may succeed only if all the 
residuals F^^'> {Uj-i , Xj, tj) are sufficiently small, or at least 
some of them. Thus, the changes by the discrete switches 
should not be too radical, which is the main limitation of 
the modihed method. 

4. TEST PROBLEM 


We consider a minimum-time problem on the two- 
dimensional plane from a state (xo,j/o) to a state (xf,yf) 
with inequality constraints. The system dynamics is gov¬ 
erned by the system of differential equations 


X 


{Ax + B) cosu 

y. 


{Ax + B) sinu 


The control variable u is subject to an inequality con¬ 
straint: u stays within the band c„ — r„ < u < c„ + r„. 
Following Ohtsuka (2004) we introduce a slack variable Ug 
and replace the inequality constraint by the equality 

C{u, Ud) = {u- c„)^ + ul-r‘i = t). 


The state is forced to pass through the point {xf,yf) at 
time t = tf by imposing two terminal constraints 


\p{x,y,p) 


X-Xf 

y-vf 


= 0 . 


The objective is to minimize the performance index 
J = (j){p)+ J L{x,y,u,Us,p)dt', 


where 

(j)[p) = p = tf - to, L{x, y, u, Us,p) = -WsUg. 


The term (/(p) is responsible for the shortest time to 
destination, and the function L serves to stabilize the slack 
variable Ug. 


For convenience, we change the time variable t within the 
horizon by the new time r = {t — to)/{tf — to), which runs 
over the interval [0,1]. 

The corresponding discretized finite-horizon problem on 
a uniform grid Ti uses the following data structures and 
computations: 


Ti = iAr, where i = 0,1,..., A^, and At = 1/A; 


the participating variables are the state 


Vt 


the 


costate 


Aiy 


, the control 


Ui 

Usi 


the Lagrange 


multipliers pi and 


Vl 

V2 



• the system dynamics is governed by the equations 
Xi+i = Xi + ATp{Axi + B) cos Mi, 


yi+i = j/i + Arp^Axi + B) sinui, 
where i = 0,1 ,..., — 1; 

• the costate is computed by the backward recursion 

(Ai^at = \2,N = V 2 ) 

Ai,i = Ai,i+i - ArpA(cosMjAiy+i + sinMiA2,i+i), 
A 2 ,i = A 2 ,i+ 1 , 

where i = iV — 1, iV — 2,..., 0; 
the nonlinear equation F{U,xoAo) = 0; where 

U = [mq, - . . , UN-1, Ms,0j • ■ ■ , Us,N-l, 

P0,...,fJ.N_l,iyi,I^2,p], 
has the following rows from the top to bottom: 
At[p{Axi + B) (- sinMiAi,i+i + cosMiA2,j+i) 

“t“ 2 {Ui C^) /ii] — 0 

{ Arp [2piUsi - Wsp] = 0 
{ Arp [(m, - Cu)^ + u% -rl]=Q 

J xn — Xf = 0 
\ VN -yj = 0 

N-1 


Ar{ ^ {Axi + i?)(cosMAi,i+i + sinMiA2,i+i) 

- WsUsi} + 1 = 0 . 


i=0 


5. NUMERICAL RESULTS 

We set g = 3 and consider q cases of the system dynamics 
simultaneously. The cases are determined by the three 
pairs of constants: {Ai,Bi) = (0.97,1.), (A 2 ,i? 2 ) = 
(0.9,1.05), (AsjBs) = (1.1,0.9). Other parameters are the 
same in all cases: the end points of the computed trajectory 
are (xo,yo) = (OjO) and (xf,yf) = (1,1); the constants in 
the inequality constraint for the control are c„ = 0.8 and 
r„ = 0.2; Ws = 0.005. 

The number of grid points on the horizon is A = 20, 
the time step of the dynamic system is At = 1/200, the 
numerical differentiation step is h = 10“®. 

The value of U at time to is approximated by the MATLAB 
function f solve with a special initial guess. 

We use the GMRES method without restarts implemented 
in MATLAB. The number of GMRES iterations does not 
exceed 30, and the absolute tolerance of the GMRES 
iterations equals 10“®. 

We apply a simple preconditioning strategy as follows. 
The exact Jacobian Fu is computed periodically at time 
instances with the period 0.2. Then the LU factorization 
of the Jacobian is used as the preconditioner until the next 
time when it is recomputed. 


k = 1, and last 79 steps with k = 3. The residual norm 
11 + 112 is shown in Eigure 4. 

We observe in Eigure 2 that during a switch to another 
pair of (Ak,Bk), the control u(t) undergoes abrupt change. 
The corresponding deterioration of the norm || + ||2 at 
these points is seen in Figure 4. The method continues to 
work while the deterioration is sufficiently small. A robust 
implementation of our method should verify || + ||2 when 
switching between discrete parameters. 

Figures 5 and 6 show the number of GMRES iterations 
in the non-preconditioned and preconditioned variants, 
respectively. The variant without preconditioning uses 
4920 iterations in total. The variant with preconditioning 
uses 2216 iterations in total. 



Fig. 1. The computed trajectory (x,y). 
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Figure 1 displays the computed trajectory for the test 
problem with automatic switches in the system dynamics. 
Time to destination along this trajectory at the initial 
point (xo,yo) is 0.974. Figure 2 plots the control u(t) 
computed by our method. Figure 3 shows how the system 
dynamics switch between (Ak,Bk) along the trajectory: 
first 95 steps are executed with k = 2, then 18 steps with 


Fig. 2. The computed control u{t). 


6 . GONGLUSION 

The numerical method, developed for nonlinear MPG 
problems in Knyazev et al. (2015), can be used in cases, 










Fig. 3. The number k of the chosen pair {Ak,Bk). 



Fig. 4. The residual norm ||F'|| 2 - 

when an ensemble of near solutions for the finite-horizon 
prediction have to be computed simultaneously. The re¬ 
duction of computing time may be achieved by using 
parallel processors for each prediction and/or by using a 
single preconditioner for the whole ensemble. 
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